
#pragma config(Hubs,  S1, HTMotor,  HTMotor,  HTMotor,  HTServo)
#pragma config(Sensor, S2,     IR1200,         sensorHiTechnicIRSeeker1200)
#pragma config(Motor,  motorA,           ,             tmotorNXT, openLoop)
#pragma config(Motor,  motorB,           ,             tmotorNXT, openLoop)
#pragma config(Motor,  motorC,           ,             tmotorNXT, openLoop)
#pragma config(Motor,  mtr_S1_C1_1,     FL,            tmotorTetrix, openLoop, reversed)
#pragma config(Motor,  mtr_S1_C1_2,     BL,            tmotorTetrix, openLoop, reversed)
#pragma config(Motor,  mtr_S1_C2_1,     FR,            tmotorTetrix, openLoop)
#pragma config(Motor,  mtr_S1_C2_2,     BR,            tmotorTetrix, openLoop)
#pragma config(Motor,  mtr_S1_C3_1,     Lift,          tmotorTetrix, openLoop)
#pragma config(Motor,  mtr_S1_C3_2,     Arm,           tmotorTetrix, openLoop)
#pragma config(Servo,  srvo_S1_C4_1,    WristLift,            tServoStandard)
#pragma config(Servo,  srvo_S1_C4_2,    servo2,               tServoNone)
#pragma config(Servo,  srvo_S1_C4_3,    servo3,               tServoNone)
#pragma config(Servo,  srvo_S1_C4_4,    servo4,               tServoNone)
#pragma config(Servo,  srvo_S1_C4_5,    servo5,               tServoNone)
#pragma config(Servo,  srvo_S1_C4_6,    servo6,               tServoNone)
//*!!Code automatically generated by 'ROBOTC' configuration wizard               !!*//
int BeaconPost=0;
void Forward (int power, int duration)						// Positive power means wheels going forward
{
	motor[FL] = power;
	motor[FR] = power;
	motor[BR] = power;
	motor[BL] = power;
	wait1Msec(duration);
}

void Backward (int power, int duration)
{
	motor[FL] = -1*power;
	motor[FR] = -1*power;
	motor[BR] = -1*power;
	motor[BL] = -1*power;
	wait1Msec(duration);
}

void turnL (int power, int duration)
{
	motor[FL] = -1*power;
	motor[FR] = power;
	motor[BR] = power;
	motor[BL] = -1*power;
	wait1Msec(duration);
}

void turnR (int power, int duration)
{
	motor[FL] = power;
	motor[FR] = -1*power;
	motor[BL] = power;
	motor[BR] = -1*power;
	wait1Msec(duration);
}
void FindBeaconPost(void)
{
	if (SensorValue[IR1200] == 2)
	{
		BeaconPost = 2;
		nxtDisplayCenteredBigTextLine(3, "%d, %d", SensorValue[IR1200],BeaconPost);
	}
	else if (SensorValue[IR1200] > 2)
	{
		BeaconPost = 3;
		nxtDisplayCenteredBigTextLine(3, "%d, %d", SensorValue[IR1200],BeaconPost);
	}
	else if (SensorValue[IR1200] < 2)
	{
		BeaconPost = 1;
		nxtDisplayCenteredBigTextLine(3, "%d, %d", SensorValue[IR1200],BeaconPost);
	}
	else
	{
		BeaconPost = 0;
		nxtDisplayCenteredBigTextLine(3, "%d, %d", SensorValue[IR1200],BeaconPost);
	}
}
 void GoToBeacon(void)
{
	if (BeaconPost == 1)
	{
		//EncoderMovementDriftL(50, 1608);
		turnL(75,400);
		Forward(75,750);
	}
	else if(BeaconPost == 2)
	{
		//Here line following will be inserted.
	}
	else if(BeaconPost == 3)
	{
		//EncoderMovementDriftR(50, 1608);
		turnR(75,400);
		Forward(75,750);

	}
}


void Stop ()
{
	motor[FL] = 0;
	motor[BR] = 0;
	motor[FR] = 0;
	motor[BL] = 0;
	wait1Msec(100);
}

	task main()
	{
		Stop();
		Forward(80,2000);
		Stop();
		turnL(30,300);
		FindBeaconPost();
		Stop();
		GoToBeacon();
		Stop();
	}
